/*
 * Copyright (c) 2024 iSOFT INFRASTRUCTURE SOFTWARE CO., LTD.
 * easyAda is licensed under Mulan PubL v2.
 * You can use this software according to the terms and conditions of the Mulan PubL v2.
 * You may obtain a copy of Mulan PubL v2 at:
 *          http://license.coscl.org.cn/MulanPubL-2.0
 * THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
 * EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
 * MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
 * See the Mulan PubL v2 for more details.
 */

#define BEGIN_FUNC(_name)       \
    .global _name;              \
    .type   _name, %function;   \
_name:

#define END_FUNC(_name)         \
    .size _name, .-_name

#define EXEC_EL1           (0b01 << 2)
#define EXEC_EL2           (0b10 << 2)

#define ICH_HCR_EL2             S3_4_C12_C11_0
#define ICC_SRE_EL2             S3_4_C12_C9_5

BEGIN_FUNC(arm64_elX_to_el1)
    mrs x9, CurrentEL

    cmp x9, EXEC_EL1
    beq .in_el1
    cmp x9, EXEC_EL2
    beq .In_el2

.In_el2:
    mrs x10, cnthctl_el2
    orr x10, x10, (1 | 2)
    msr cnthctl_el2, x10

    msr cntvoff_el2, xzr

    msr vttbr_el2, xzr

    mov x10, #0x33ff
    msr cptr_el2, x10

    mrs x10, cntkctl_el1
    orr x10, x10, (1)
    msr cntkctl_el1, x10

    mov x10, (0b11 << 20)
    msr cpacr_el1, x10

    mrs x9, id_aa64pfr0_el1
    and x9, x9, (0b1111 << 24)
    cbz x9, .no_gic_sr

    mrs x9, ICC_SRE_EL2
    mov x10, #0x9
    orr x9, x9, x10
    msr ICC_SRE_EL2, x9

    msr ICH_HCR_EL2, xzr

.no_gic_sr:
    mov x9, (1 << 31)
    msr hcr_el2, x9

    adr x9, .in_el1
    msr elr_el2, x9
    mov x9, #0x3c5
    msr spsr_el2, x9

    isb
    eret

.in_el1:
    ret
END_FUNC(arm64_elX_to_el1)
